# Numpy wrapper or pure-Python Matrix objects in absence of Numpy
# Mark Wilson, June 2010

#
# TODO: type safety and a better interface, zero matrices on make_rot calls

import math

has_np = True
#has_np = False # Uncomment to get pure-Python implementation
try:
	import numpy as np
except:
	has_np = False
	
if has_np:
	class Matrix:
		def __init__(self, m, n):
			self.m = m
			self.n = n
			self.matrix = np.asmatrix(np.zeros([m,n]))
			
		def __repr__(self):
			return str(self.matrix.tolist())
			
		def __str__(self):
			return str(repr(self))
		
		def mul(self, other):
			if self.n != other.m:
				return None
			
			result = Matrix(self.m, other.n)
			result.matrix = self.matrix*other.matrix
			return result
		
		def set(self, i1, i2, val):
			if i1 < 0 or i1 >= self.m or i2 < 0 or i2 >= self.n:
				return
			self.matrix[i1,i2] = val
		
		def get(self, i1, i2):
			if i1 < 0 or i1 >= self.m or i2 < 0 or i2 >= self.n:
				return None
			return self.matrix[i1,i2]
			
		def get_dim(self):
			return (self.m,self.n)
		
		def make_rot_x(self, rads):
			if self.m != 3 or self.n != 3:
				return
			self.matrix[0,0] = 1.0
			self.matrix[1,1] = math.cos(rads)
			self.matrix[2,1] = math.sin(rads)
			self.matrix[1,2] = -math.sin(rads)
			self.matrix[2,2] = math.cos(rads)
		
		def make_rot_y(self, rads):
			if self.m != 3 or self.n != 3:
				return
			self.matrix[0,0] = math.cos(rads)
			self.matrix[2,0] = -math.sin(rads)
			self.matrix[1,1] = 1.0
			self.matrix[0,2] = math.sin(rads)
			self.matrix[2,2] = math.cos(rads)
		
		def make_rot_z(self, rads):
			if self.m != 3 or self.n != 3:
				return
			self.matrix[0,0] = math.cos(rads)
			self.matrix[1,0] = math.sin(rads)
			self.matrix[0,1] = -math.sin(rads)
			self.matrix[1,1] = math.cos(rads)
			self.matrix[2,2] = 1.0
			
		def inverse(self):
			if self.m != 3 or self.n != 3:
				return None
			mat = Matrix(3,3)
			mat.matrix = self.matrix.I
			return mat
	
	
# no numpy	
else:
	class Matrix:
		def __init__(self, m, n):
			self.m = m
			self.n = n
			self.matrix = [[1.0 if j == i else 0.0 for j in range(n)] for i in range(m)]
			
		def __repr__(self):
			return str(self.matrix)
			
		def __str__(self):
			return str(repr(self))
			
		def mul(self, other):
			if self.n != other.m:
				return None
			
			result = Matrix(self.m, other.n)
			for i in range(self.m):
				for j in range(other.n):
					result.matrix[i][j] = \
						sum([self.matrix[i][k]*other.matrix[k][j] \
								for k in range(self.n)])
			return result
		
		def set(self, i1, i2, val):
			if i1 < 0 or i1 >= self.m or i2 < 0 or i2 >= self.n:
				return
			self.matrix[i1][i2] = val
		
		def get(self, i1, i2):
			if i1 < 0 or i1 >= self.m or i2 < 0 or i2 >= self.n:
				return None
			return self.matrix[i1][i2]
			
		def get_dim(self):
			return (self.m, self.n)
		
		def make_rot_x(self, rads):
			if self.m != 3 or self.n != 3:
				return
			for col in self.matrix:
				for row in col:
					row = 0.0
			self.matrix[0][0] = 1.0
			self.matrix[1][1] = math.cos(rads)
			self.matrix[2][1] = math.sin(rads)
			self.matrix[1][2] = -math.sin(rads)
			self.matrix[2][2] = math.cos(rads)
		
		def make_rot_y(self, rads):
			if self.m != 3 or self.n != 3:
				return
			for col in self.matrix:
				for row in col:
					row = 0.0
			self.matrix[0][0] = math.cos(rads)
			self.matrix[2][0] = -math.sin(rads)
			self.matrix[1][1] = 1.0
			self.matrix[0][2] = math.sin(rads)
			self.matrix[2][2] = math.cos(rads)
		
		def make_rot_z(self, rads):
			if self.m != 3 or self.n != 3:
				return
			for col in self.matrix:
				for row in col:
					row = 0.0
			self.matrix[0][0] = math.cos(rads)
			self.matrix[1][0] = math.sin(rads)
			self.matrix[0][1] = -math.sin(rads)
			self.matrix[1][1] = math.cos(rads)
			self.matrix[2][2] = 1.0
			
		def det(self):
			if self.m != 3 or self.n != 3:
				print "m,n:", m, n
				print "Returning None for det"
				return None
			m = self.matrix
			return m[0][0]*m[1][1]*m[2][2] + m[0][1]*m[1][2]*m[2][0] \
					+ m[0][2]*m[1][0]*m[2][1] - m[0][0]*m[1][2]*m[2][1] \
					- m[0][1]*m[1][0]*m[2][2] - m[0][2]*m[1][1]*m[2][0]
			
		def inverse(self):
			print "Inverting..."
			if self.m != 3 or self.n != 3:
				print "m,n:", m, n
				print "Returning None for inverse"
				return None
			mat = Matrix(3,3)
			m = self.matrix
			d = 1.0 / self.det()
			mat.set(0,0, d*(m[1][1]*m[2][2] - m[1][2]*m[2][1]))
			mat.set(1,0, d*(m[1][2]*m[2][0] - m[1][0]*m[2][2]))
			mat.set(2,0, d*(m[1][0]*m[2][1] - m[1][1]*m[2][0]))
			mat.set(0,1, d*(m[0][2]*m[2][1] - m[0][1]*m[2][2]))
			mat.set(1,1, d*(m[0][0]*m[2][2] - m[0][2]*m[2][0]))
			mat.set(2,1, d*(m[0][1]*m[2][0] - m[0][0]*m[2][1]))
			mat.set(0,2, d*(m[0][1]*m[1][2] - m[0][2]*m[1][1]))
			mat.set(1,2, d*(m[0][2]*m[1][0] - m[0][0]*m[1][2]))
			mat.set(2,2, d*(m[0][0]*m[1][1] - m[0][1]*m[1][0]))
			print "Returning", mat, ".."
			return mat